/*
 * HeartRateInterrupt.cpp
 *
 *  Created on: 14/04/2013
 *      Author: roger
 */

#include "HeartRateInterrupt.h"
#include <Hardware.h>

#define PULSE_LED LED1_PIN

HeartRateInterrupt::HeartRateInterrupt(long startTime) {
	m_counter = 0;
	m_lastPeakTime = startTime;
	m_runningTime = startTime;
	m_bpm = 0;
	m_counter = 0;
	m_lastRunningTotal = 0;
	m_lastSign = 0;
	m_high = 0;
	m_low = 0;
	m_troughTime = -1;
	m_troughBottom = 0;
	m_slope = 0;
	m_my = 0.4F;
	m_effects = false;
	m_lastInterval = 0.0F;
}
HeartRateInterrupt::~HeartRateInterrupt() {
}

int HeartRateInterrupt::getCurrentBPM() {
	return m_bpm;
}
void HeartRateInterrupt::setLogRawData(bool logRawData) {
	m_logRawData = logRawData;
	if (logRawData) {
		m_runningTime = 0;
	} else {
		digitalWrite(PULSE_LED, LOW);
	}
}

void HeartRateInterrupt::calculate(int signal) {

	digitalWrite(PULSE_LED, LOW);
	noTone(TONE_PIN);

	int topIndex = PREVBUFSIZE-1;
	m_runningTime += 2; // add 2ms to the running time.
	if (m_counter == 0) {
		for (int i = 0; i <= topIndex; i++) {
			m_prev[i] = signal;
		}
		m_lastRunningTotal = signal;
		m_lastSign = 0;
		m_high = signal;
		m_low = signal;
	}
	m_counter++;

	// This gathers the high and low points
	if (signal > m_high) {
		m_high = signal;
	}
	if (signal < m_low) {
		m_low = signal;
	}

	// Smooth out the signal with a rolling average
	int runningTotal = 0;
	for (int i = 0; i <= (PREVBUFSIZE-2); i++) { // shift data in the rate array
		m_prev[i] = m_prev[i + 1]; // and drop the oldest IBI value
		runningTotal += m_prev[i]; // add up the 9 oldest IBI values
	}
	m_prev[topIndex] = signal; // add the latest IBI to the rate array
	runningTotal += m_prev[topIndex]; // add the latest IBI to runningTotal
	runningTotal /= PREVBUFSIZE; // average the last 10 IBI values

	if (m_logRawData) {
		int x = m_counter % (Graphics.width()*8);
		int y = 230-(((runningTotal-440)*0.20F)+80);
		y = constrain(y, 0, 230);
		if (x == 0) {
			Graphics.fillRect(0,0,Graphics.width(),Graphics.height(),WHITE);
			Graphics.setCursor(120,100);
			uint16_t colour = GREEN;
			if (m_bpm < configuration.getLowPulse()) {
				colour = BLUE;
			}
			if (m_bpm > configuration.getHighPulse()) {
				colour = RED;
			}
			Graphics.setTextColor(colour);
			Graphics.setTextSize(3);
			Graphics.print(m_bpm);
		}
		Graphics.drawPixel(x/8,y,BLACK);
	}

	// see if we just passed a peak.
	int sign = runningTotal - m_lastRunningTotal;
	if (signal > HIGH_CUTOFF && sign < 0 && m_slope > SLOPE && m_troughTime > -1) {
		// we just passed a peak because the sign changed
		float dy = runningTotal - m_troughBottom;
		float dx = m_runningTime - m_troughTime;
		float slope = dy/dx;
		if (slope < 2) {
			// Slope has to be steep enough to qualify for a peak
			long peakTime = m_runningTime;
			float interval = peakTime - m_lastPeakTime;
			m_lastPeakTime = peakTime;
			float bpm = 60000/interval;
			m_bpm = (int)bpm;
			m_slope = 0;
			m_troughTime = -1;
			if (m_effects) {
				digitalWrite(PULSE_LED, HIGH);
				if (m_bpm < configuration.getLowPulse()) {
					tone(TONE_PIN, 3136,31);
				}
				if (m_bpm > configuration.getHighPulse()) {
					tone(TONE_PIN, 4186,30);
				}
			}
			poincarePlot(interval, m_lastInterval);
			m_lastInterval = interval;
		}
	}
	if (runningTotal < LOW_CUTOFF && sign > 0 && m_lastSign <=0 && m_slope <=0) {
		// Should be the bottom of a trough here
		m_troughBottom = runningTotal;
		m_troughTime = m_runningTime;
	}
	if (sign > 0) {
		m_slope++;
	} else if (sign < 0) {
		m_slope = 0;
	}
	m_lastSign = sign;
	m_lastRunningTotal = runningTotal;
}

void HeartRateInterrupt::poincarePlot(float interval, float lastInterval) {
	int y = (interval-800)*0.4;
	int x = (lastInterval-800)*0.6;
	if (m_logRawData) {
		Graphics.fillCircle(constrain(x,0,320),constrain(y,0,240),2,BLACK);
	}
}

